/**
  ****************************(C) COPYRIGHT 2025 ROBOT-Z****************************
  * @file       gimbal_behaviour.c/h
  * @brief      2025赛季哨兵，根据遥控器的值，决定云台行为
  * @note
  * @history
  *  Version    Date            Author          Modification
  *  V1.0.0     Dec-26-2018     RM              1. done
  *  V1.1.0     Nov-11-2019     RM              1. 添加一些注释
  *  V2.0.0     Jan-1-2024      申欣宇           1. 优化云台行为逻辑
  *  V3.0.0     Dev-20-2024     吴森桦           1. 添加自瞄模式
  *                             冉文治           2. 添加巡逻模式
  *                                             3. 删除不必要的模式，简化代码
  *
  @verbatim
  ==============================================================================
    如果要添加一个新的行为模式
    1.首先，在gimbal_behaviour.h文件中， 添加一个新行为名字在 gimbal_behaviour_e
    erum
    {
        ...
        ...
        GIMBAL_XXX_XXX, // 新添加的
    }gimbal_behaviour_e,
    2. 实现一个新的函数 gimbal_xxx_xxx_control(fp32 *yaw, fp32 *pitch, gimbal_control_t *gimbal_control_set);
        "yaw, pitch" 参数是云台运动控制输入量
        第一个参数: 'yaw' 通常控制yaw轴移动,通常是角度增量,正值是逆时针运动,负值是顺时针
        第二个参数: 'pitch' 通常控制pitch轴移动,通常是角度增量,正值是逆时针运动,负值是顺时针
        在这个新的函数, 你能给 "yaw"和"pitch"赋值想要的参数
    3.  在"gimbal_behavour_set"这个函数中，添加新的逻辑判断，给gimbal_behaviour赋值成GIMBAL_XXX_XXX
        在gimbal_behaviour_mode_set函数最后，添加"else if(gimbal_behaviour == GIMBAL_XXX_XXX)" ,然后选择一种云台控制模式
    4.  在"gimbal_behaviour_control_set" 函数的最后，添加
        else if(gimbal_behaviour == GIMBAL_XXX_XXX)
        {
            gimbal_xxx_xxx_control(&rc_add_yaw, &rc_add_pit, gimbal_control_set);
        }
  ==============================================================================
  @endverbatim
  ****************************(C) COPYRIGHT 2025 ROBOT-Z****************************
  */

#include "gimbal_behaviour.h"
#include "chassis_task.h"
#include "arm_math.h"
#include "detect_task.h"
#include "math.h"
#include "stdio.h"
#include "referee.h"

/**
  * @brief          遥控器的死区判断，因为遥控器的拨杆在中位的时候，不一定为0
  * @author         吴森桦
  * @param          输入的遥控器值
  * @param          输出的死区处理后遥控器值
  * @param          死区值
  */
#define rc_deadband_limit(input, output, dealine)        \
    {                                                    \
        if ((input) > (dealine) || (input) < -(dealine)) \
        {                                                \
            (output) = (input);                          \
        }                                                \
        else                                             \
        {                                                \
            (output) = 0;                                \
        }                                                \
    }

/**
  * @brief          云台行为状态机设置
  * @author         吴森桦
  * @param          gimbal_mode_set 云台数据指针
  * @retval         none
  */
static void gimbal_behavour_set(gimbal_control_t *gimbal_mode_set);

/**
  * @brief          当云台行为模式是GIMBAL_ZERO_FORCE, 这个函数会被调用,云台控制模式是raw模式.原始模式意味着设定值会直接发送到CAN总线上,这个函数将会设置所有为0
  * @author         吴森桦
  * @param          yaw 发送yaw电机的原始值，会直接通过can 发送到电机
  * @param          pitch 发送pitch电机的原始值，会直接通过can 发送到电机
  * @param          gimbal_control_set 云台数据指针
  * @retval         none
  */
static void gimbal_zero_force_control(fp32 *yaw, fp32 *pitch, gimbal_control_t *gimbal_control_set);

/**
  * @brief          云台初始化控制，电机是陀螺仪角度控制，云台先抬起pitch轴，后旋转yaw轴
  * @author         吴森桦
  * @param          yaw 轴角度控制，为角度的增量 单位 rad
  * @param          pitch 轴角度控制，为角度的增量 单位 rad
  * @param          gimbal_control_set 云台数据指针
  * @retval         返回空
  */
static void gimbal_init_control(fp32 *yaw, fp32 *pitch, gimbal_control_t *gimbal_control_set);

/**
  * @brief          云台陀螺仪控制，电机是陀螺仪角度控制，
  * @author         吴森桦
  * @param          yaw yaw轴角度控制，为角度的增量 单位 rad
  * @param          pitch pitch轴角度控制，为角度的增量 单位 rad
  * @param          gimbal_control_set 云台数据指针
  * @retval         none
  */
static void gimbal_absolute_angle_control(fp32 *yaw, fp32 *pitch, gimbal_control_t *gimbal_control_set);

/**
  * @brief          云台编码值控制，电机是相对角度控制，
  * @author         吴森桦
  * @param          yaw: yaw轴角度控制，为角度的增量 单位 rad
  * @param          pitch: pitch轴角度控制，为角度的增量 单位 rad
  * @param          gimbal_control_set: 云台数据指针
  * @retval         none
  */
static void gimbal_relative_angle_control(fp32 *yaw, fp32 *pitch, gimbal_control_t *gimbal_control_set);

/**
  * @brief          云台进入遥控器无输入控制，电机是相对角度控制，
  * @author         RM
  * @param          yaw: yaw轴角度控制，为角度的增量 单位 rad
  * @param          pitch: pitch轴角度控制，为角度的增量 单位 rad
  * @param          gimbal_control_set:云台数据指针
  * @retval         none
  */
static void gimbal_motionless_control(fp32 *yaw, fp32 *pitch, gimbal_control_t *gimbal_control_set);

/**
  * @brief          自瞄模式
  * @author         吴森桦
  * @param          yaw yaw轴角度控制，为角度的增量 单位 rad
  * @param          pitch pitch轴角度控制，为角度的增量 单位 rad
  * @param          gimbal_control_set 云台数据指针
  * @retval         none
  */
static void gimbal_auto_control(fp32 *yaw, fp32 *pitch, gimbal_control_t *gimbal_control_set);

// 云台行为状态机
gimbal_behaviour_e gimbal_behaviour = GIMBAL_ZERO_FORCE;

/**
  * @brief          被gimbal_set_mode函数调用在gimbal_task.c,云台行为状态机以及电机状态机设置
  * @author         吴森桦
  * @param          gimbal_mode_set 云台数据指针
  * @retval         none
  */
void gimbal_behaviour_mode_set(gimbal_control_t *gimbal_mode_set)
{
    if (gimbal_mode_set == NULL)
    {
        return;
    }

    // 云台行为状态机设置
    gimbal_behavour_set(gimbal_mode_set);
    // 根据云台行为状态机设置电机状态机
    if (gimbal_behaviour == GIMBAL_ZERO_FORCE)
    {
        gimbal_mode_set->gimbal_yaw_motor.gimbal_motor_mode = GIMBAL_MOTOR_RAW;
        gimbal_mode_set->gimbal_pitch_motor.gimbal_motor_mode = GIMBAL_MOTOR_RAW;
    }
    else if (gimbal_behaviour == GIMBAL_INIT)
    {
        gimbal_mode_set->gimbal_yaw_motor.gimbal_motor_mode = GIMBAL_MOTOR_ENCONDE;
        gimbal_mode_set->gimbal_pitch_motor.gimbal_motor_mode = GIMBAL_MOTOR_GYRO;
    }
    else if (gimbal_behaviour == GIMBAL_ABSOLUTE_ANGLE)
    {
        gimbal_mode_set->gimbal_yaw_motor.gimbal_motor_mode = GIMBAL_MOTOR_GYRO;
        gimbal_mode_set->gimbal_pitch_motor.gimbal_motor_mode = GIMBAL_MOTOR_ENCONDE;
    }
    else if (gimbal_behaviour == GIMBAL_RELATIVE_ANGLE)
    {
        gimbal_mode_set->gimbal_yaw_motor.gimbal_motor_mode = GIMBAL_MOTOR_ENCONDE;
        gimbal_mode_set->gimbal_pitch_motor.gimbal_motor_mode = GIMBAL_MOTOR_ENCONDE;
    }
    else if (gimbal_behaviour == GIMBAL_MOTIONLESS)
    {
        gimbal_mode_set->gimbal_yaw_motor.gimbal_motor_mode = GIMBAL_MOTOR_ENCONDE;
        gimbal_mode_set->gimbal_pitch_motor.gimbal_motor_mode = GIMBAL_MOTOR_ENCONDE;
    }
    else if (gimbal_behaviour == GIMBAL_CV)
    {
        gimbal_mode_set->gimbal_yaw_motor.gimbal_motor_mode = GIMBAL_MOTOR_CV;
        gimbal_mode_set->gimbal_pitch_motor.gimbal_motor_mode = GIMBAL_MOTOR_CV;
    }
}

/**
  * @brief          云台行为控制，根据不同行为采用不同控制函数
  * @author         吴森桦
  * @param          add_yaw 设置的yaw角度增加值，单位 rad
  * @param          add_pitch 设置的pitch角度增加值，单位 rad
  * @param          gimbal_control_set 云台数据指针
  * @retval         none
  */
void gimbal_behaviour_control_set(fp32 *add_yaw, fp32 *add_pitch, gimbal_control_t *gimbal_control_set)
{

    if (add_yaw == NULL || add_pitch == NULL || gimbal_control_set == NULL)
    {
        return;
    }
    if (gimbal_behaviour == GIMBAL_ZERO_FORCE)
    {
        gimbal_zero_force_control(add_yaw, add_pitch, gimbal_control_set);
    }
    else if (gimbal_behaviour == GIMBAL_INIT)
    {
        gimbal_init_control(add_yaw, add_pitch, gimbal_control_set);
    }
    else if (gimbal_behaviour == GIMBAL_ABSOLUTE_ANGLE)
    {
        gimbal_absolute_angle_control(add_yaw, add_pitch, gimbal_control_set);
    }
    else if (gimbal_behaviour == GIMBAL_RELATIVE_ANGLE)
    {
        gimbal_relative_angle_control(add_yaw, add_pitch, gimbal_control_set);
    }
    else if (gimbal_behaviour == GIMBAL_MOTIONLESS)
    {
        gimbal_motionless_control(add_yaw, add_pitch, gimbal_control_set);
    }
    else if(gimbal_behaviour == GIMBAL_CV)
    {
        gimbal_auto_control(add_yaw, add_pitch, gimbal_control_set);
    }
}

/**
  * @brief          云台在某些行为下，需要底盘不动
  * @author         吴森桦
  * @param          none
  * @retval         1: no move 0:normal
  */
bool_t gimbal_cmd_to_chassis_stop(void)
{
    if (gimbal_behaviour == GIMBAL_INIT || gimbal_behaviour == GIMBAL_ZERO_FORCE)
    {
        return 1;
    }
    else
    {
        return 0;
    }
}

/**
  * @brief          云台在某些行为下，需要射击停止
  * @author         吴森桦
  * @param          none
  * @retval         1: no move 0:normal
  */
bool_t gimbal_cmd_to_shoot_stop(void)
{
    if (gimbal_behaviour == GIMBAL_INIT || gimbal_behaviour == GIMBAL_ZERO_FORCE)
    {
        return 1;
    }
    else
    {
        return 0;
    }
}

/**
  * @brief          云台行为状态机设置
  * @author         吴森桦
  * @param          gimbal_mode_set 云台数据指针
  * @retval         none
  */
static void gimbal_behavour_set(gimbal_control_t *gimbal_mode_set)
{
    if (gimbal_mode_set == NULL)
    {
        return;
    }

    // 初始化模式判断是否到达中值位置
    if (gimbal_behaviour == GIMBAL_INIT)
    {
        static uint16_t init_time = 0;
        static uint16_t init_stop_time = 0;
        init_time++;
        if ((fabs(gimbal_mode_set->gimbal_yaw_motor.relative_angle - INIT_YAW_SET) < GIMBAL_INIT_ANGLE_ERROR
            && fabs(gimbal_mode_set->gimbal_pitch_motor.absolute_angle - INIT_PITCH_SET) < GIMBAL_INIT_ANGLE_ERROR))
        {

            if (init_stop_time < GIMBAL_INIT_STOP_TIME)
            {
                init_stop_time++;
            }
        }
        else
        {
            if (init_time < GIMBAL_INIT_TIME)
            {
                init_time++;
            }
        }
        // 超过初始化最大时间，或者已经稳定到中值一段时间，退出初始化状态开关打下档，或者掉线
        if (init_time < GIMBAL_INIT_TIME && init_stop_time < GIMBAL_INIT_STOP_TIME
            && !switch_is_down(gimbal_mode_set->gimbal_rc_ctrl->rc.s[GIMBAL_MODE_CHANNEL])
            && !toe_is_error(DBUS_TOE))
        {
            return;
        }
        else
        {
            init_stop_time = 0;
            init_time = 0;
        }
    }

    // 开关控制 云台状态
    if (switch_is_down(gimbal_mode_set->gimbal_rc_ctrl->rc.s[GIMBAL_MODE_CHANNEL]))
    {
        gimbal_behaviour = GIMBAL_ZERO_FORCE;
    }
    else if (switch_is_mid(gimbal_mode_set->gimbal_rc_ctrl->rc.s[GIMBAL_MODE_CHANNEL]))
    {
        gimbal_behaviour = GIMBAL_ABSOLUTE_ANGLE;
    }
    else if (switch_is_up(gimbal_mode_set->gimbal_rc_ctrl->rc.s[GIMBAL_MODE_CHANNEL]))
    {
        if (chassis_move.chassis_RC->rc.ch[4] == -660)
        {
            if (gimbal_mode_set->cv_ctrl->state)
            {
                gimbal_behaviour = GIMBAL_CV;
            }
            else
            {
                gimbal_behaviour = GIMBAL_ABSOLUTE_ANGLE;
            }
        }
        else
        {
            if (gimbal_mode_set->cv_ctrl->state)
            {
                gimbal_behaviour = GIMBAL_CV;
            }
            else
            {
                gimbal_behaviour = GIMBAL_RELATIVE_ANGLE;
            }
        }
    }
    else
    {
        gimbal_behaviour = GIMBAL_ZERO_FORCE;
    }

    if (toe_is_error(YAW_GIMBAL_MOTOR_TOE) && toe_is_error(YAW_GIMBAL_MOTOR_TOE))
    {
        gimbal_behaviour = GIMBAL_ZERO_FORCE;
    }

    if (toe_is_error(DBUS_TOE) && game_state_t.game_progress != 4)
    {
        gimbal_behaviour = GIMBAL_ZERO_FORCE;
    }
    //判断进入init状态机
    {
        static gimbal_behaviour_e last_gimbal_behaviour = GIMBAL_ZERO_FORCE;
        if (last_gimbal_behaviour == GIMBAL_ZERO_FORCE && gimbal_behaviour != GIMBAL_ZERO_FORCE)
        {
            gimbal_behaviour = GIMBAL_INIT;
        }
        last_gimbal_behaviour = gimbal_behaviour;
    }
}

/**
  * @brief          当云台行为模式是GIMBAL_ZERO_FORCE, 这个函数会被调用,云台控制模式是raw模式.原始模式意味着设定值会直接发送到CAN总线上,这个函数将会设置所有为0
  * @author         吴森桦
  * @param          yaw 发送yaw电机的原始值，会直接通过can 发送到电机
  * @param          pitch 发送pitch电机的原始值，会直接通过can 发送到电机
  * @param          gimbal_control_set 云台数据指针
  * @retval         none
  */
static void gimbal_zero_force_control(fp32 *yaw, fp32 *pitch, gimbal_control_t *gimbal_control_set)
{
    if (yaw == NULL || pitch == NULL || gimbal_control_set == NULL)
    {
        return;
    }

    *yaw = 0.0f;
    *pitch = 0.0f;
}

/**
  * @brief          云台初始化控制，电机是陀螺仪角度控制，云台先抬起pitch轴，后旋转yaw轴
  * @author         吴森桦
  * @param          yaw 轴角度控制，为角度的增量 单位 rad
  * @param          pitch 轴角度控制，为角度的增量 单位 rad
  * @param          gimbal_control_set 云台数据指针
  * @retval         返回空
  */
static void gimbal_init_control(fp32 *yaw, fp32 *pitch, gimbal_control_t *gimbal_control_set)
{
    if (yaw == NULL || pitch == NULL || gimbal_control_set == NULL)
    {
        return;
    }

    // 初始化状态控制量计算
    if (fabs(INIT_PITCH_SET - gimbal_control_set->gimbal_pitch_motor.absolute_angle) > GIMBAL_INIT_ANGLE_ERROR)
    {
        *pitch = (INIT_PITCH_SET - gimbal_control_set->gimbal_pitch_motor.absolute_angle) * GIMBAL_INIT_PITCH_SPEED;
        *yaw = 0.0f;
    }
    else
    {
        *pitch = (INIT_PITCH_SET - gimbal_control_set->gimbal_pitch_motor.absolute_angle) * GIMBAL_INIT_PITCH_SPEED;
        *yaw = (INIT_YAW_SET - gimbal_control_set->gimbal_yaw_motor.relative_angle) * GIMBAL_INIT_YAW_SPEED;
    }
}

/**
  * @brief          云台陀螺仪控制，电机是陀螺仪角度控制，
  * @author         吴森桦
  * @param          yaw yaw轴角度控制，为角度的增量 单位 rad
  * @param          pitch pitch轴角度控制，为角度的增量 单位 rad
  * @param          gimbal_control_set 云台数据指针
  * @retval         none
  */
static void gimbal_absolute_angle_control(fp32 *yaw, fp32 *pitch, gimbal_control_t *gimbal_control_set)
{
    if (yaw == NULL || pitch == NULL || gimbal_control_set == NULL)
    {
        return;
    }

    static int16_t yaw_channel = 0, pitch_channel = 0;

    rc_deadband_limit(gimbal_control_set->gimbal_rc_ctrl->rc.ch[YAW_CHANNEL], yaw_channel, RC_DEADBAND);
    rc_deadband_limit(gimbal_control_set->gimbal_rc_ctrl->rc.ch[PITCH_CHANNEL], pitch_channel, RC_DEADBAND);

    *yaw = yaw_channel * YAW_RC_SEN;
    *pitch = pitch_channel * PITCH_RC_SEN;
}

/**
  * @brief          云台编码值控制，电机是相对角度控制，巡逻模式
  * @param          yaw: yaw轴角度控制，为角度的增量 单位 rad
  * @param          pitch: pitch轴角度控制，为角度的增量 单位 rad
  * @param          gimbal_control_set: 云台数据指针
  * @retval         none
  */
static void gimbal_relative_angle_control(fp32 *yaw, fp32 *pitch, gimbal_control_t *gimbal_control_set)
{
    if (yaw == NULL || pitch == NULL || gimbal_control_set == NULL)
    {
        return;
    }

    if (gimbal_behaviour == GIMBAL_RELATIVE_ANGLE)
    {
        static fp32 dir_yaw = YAW_PATROL;
        static fp32 dir_pitch = PITCH_PATROL;

        if(gimbal_control_set->gimbal_yaw_motor.relative_angle > PI / 2)
            dir_yaw = gimbal_control_set->gimbal_yaw_patrol_speed;
        else if(gimbal_control_set->gimbal_yaw_motor.relative_angle < -PI / 2)
            dir_yaw = gimbal_control_set->gimbal_yaw_patrol_speed;

        if(gimbal_control_set->gimbal_pitch_motor.relative_angle > gimbal_control_set->gimbal_pitch_motor.max_relative_angle - 0.15f)
            dir_pitch = PITCH_PATROL;
        else if(gimbal_control_set->gimbal_pitch_motor.relative_angle < gimbal_control_set->gimbal_pitch_motor.min_relative_angle + 0.1f)
            dir_pitch = -PITCH_PATROL;

        *yaw = dir_yaw;
        *pitch = dir_pitch;
    }
    else if (gimbal_behaviour == GIMBAL_ABSOLUTE_ANGLE)
    {
        static int16_t yaw_channel = 0, pitch_channel = 0;

        rc_deadband_limit(gimbal_control_set->gimbal_rc_ctrl->rc.ch[YAW_CHANNEL], yaw_channel, RC_DEADBAND);
        rc_deadband_limit(gimbal_control_set->gimbal_rc_ctrl->rc.ch[PITCH_CHANNEL], pitch_channel, RC_DEADBAND);

        *yaw = yaw_channel * YAW_RC_SEN;
        *pitch = pitch_channel * PITCH_RC_SEN;
    }
}

/**
  * @brief          云台进入遥控器无输入控制，电机是相对角度控制，
  * @author         RM
  * @param          yaw: yaw轴角度控制，为角度的增量 单位 rad
  * @param          pitch: pitch轴角度控制，为角度的增量 单位 rad
  * @param          gimbal_control_set:云台数据指针
  * @retval         none
  */
static void gimbal_motionless_control(fp32 *yaw, fp32 *pitch, gimbal_control_t *gimbal_control_set)
{
    if (yaw == NULL || pitch == NULL || gimbal_control_set == NULL)
    {
        return;
    }
    *yaw = 0.0f;
    *pitch = 0.0f;
}

/**
  * @brief          云台进入辅瞄模式，由视觉传递的偏差角度控制。
  * @author         sxy
  * @param[in]      yaw: yaw轴角度控制，为角度的增量 单位 rad
  * @param[in]      pitch: pitch轴角度控制，为角度的增量 单位 rad
  * @param[in]      gimbal_control_set:云台数据指针
  * @retval         none
  */

static void gimbal_auto_control(fp32 *yaw, fp32 *pitch, gimbal_control_t *gimbal_control_set)
{
    if (yaw == NULL || pitch == NULL || gimbal_control_set == NULL)
    {
        return;
    }
    if (fabs(gimbal_control_set->cv_ctrl->x) < 0.0000001f || fabs(gimbal_control_set->cv_ctrl->y) < 0.0000001f)
    {
        gimbal_control_set->cv_ctrl->target = 0;
    }
    else if (fabs(gimbal_control_set->cv_ctrl->y) > 0.0000001f && fabs(gimbal_control_set->cv_ctrl->x) > 0.0000001f)
    {
        gimbal_control_set->cv_ctrl->target = 1;
        *yaw = gimbal_control_set->cv_ctrl->x;
        *pitch = gimbal_control_set->cv_ctrl->y;
    }
    abs_limit(yaw, PI);//对云台传递数据进行限幅
    abs_limit(pitch, 0.49);
}
